#include "robotModel.h"
#include "logger.h"
#include <stdio.h>
#include <iostream>
using namespace std;
using namespace Eigen;
using namespace MODEL;
void copyData(int * dst,int*src,int width,float k)
{
    for(int i=0;i<width;i++)
        dst[i]=src[i]*k;
}

void copyData(int ** dst,int** src,int width,int hight,float k)
{
    int *pdst,*psrc;
    for(int j=0;j<hight;j++)
    {
        pdst=*(dst+20*j);psrc=*(src+20*j);
        for(int i=0;i<width;i++)
            pdst[i]=psrc[i]*k;
    }
}
void copyDataBlock(int * dst,int* src,int width,int hight,float k)
{
    int *pdst,*psrc;
    for(int j=0;j<hight;j++)
    {
        pdst=(dst+20*j);psrc=(src+20*j);
        for(int i=0;i<width;i++)
            pdst[i]=psrc[i]*k;
    }
}
PoseArray initPoseArray(Vector3f v)
{
    PoseArray t;
    Matrix3f m;
    m<<1,0,0,0,1,0, 0,0,1;
    t=setPoseArray(m,v);
    return t;
}
PoseArray setPoseArray(Matrix3f T,Vector3f v)
{
    PoseArray t;
    t<<T,v,0,0,0,1;
    return t;
}
Vector3f getPose(PoseArray a)
{
    return a.block<3,1>(0,3);
}
logger mlog("robotmodel.log");
robotModel::robotModel()
{
    //ctor
    m_array=Eigen::VectorXi(20);
    m_data=m_array.data();
    for(int i=0;i<20;i++)
        m_data[i]=0;

}

robotModel::~robotModel()
{
    //dtor
}
void robotModel::setLeg(double rotateX,double rotateY,double rotateZ,double len,int leg)
{
    //record

     if(leg==LEFT)
    {
        m_info.left_leg[0]=rotateX;
        m_info.left_leg[1]=rotateY;
        m_info.left_leg[2]=rotateZ;
        m_info.left_leg[3]=len;
        m_pos_leg_left=AngleAxisf(rotateX,Vector3f::UnitX())*
        AngleAxisf(rotateY,Vector3f::UnitY())*
        AngleAxisf(rotateZ,Vector3f::UnitZ())*Vector3f(0,0,-len/2);
    }
    if(leg==RIGHT)
    {
        m_info.right_leg[0]=rotateX;
        m_info.right_leg[1]=rotateY;
        m_info.right_leg[2]=rotateZ;
        m_info.right_leg[3]=len;

        m_pos_leg_right=AngleAxisf(rotateX,Vector3f::UnitX())*
        AngleAxisf(rotateY,Vector3f::UnitY())*
        AngleAxisf(rotateZ,Vector3f::UnitZ())*Vector3f(0,0,-len/2);
    }

    len-=11.4;
    int t_data[4];
    if(leg==LEFT)
    {
        rotateX=-rotateX;
        rotateZ=-rotateZ;
    }

    double t_a=-acos(len/leg_length/2);
    t_data[0]=(t_a+rotateY)/r_p_s;
    t_data[3]=(rotateY-t_a)/r_p_s;
    t_data[2]=rotateZ/r_p_s;
    t_data[1]=rotateX/r_p_s;

    if(leg==LEFT)
        copyData(m_data+8,t_data,4,1);
    else
    if(leg==RIGHT)
        copyData(m_data+1,t_data,4,(float)-1.0);
    else
    {
    mlog.log(ERROR,"setLeg","wrong side");
    }

    updataParam(leg);
}

void robotModel::setFoot(double rotateX,double rotateY ,int leg)
{
    int t_data[2];
    if(leg==RIGHT)
        rotateY=-rotateY;

    t_data[1]=-rotateX/r_p_s;
    t_data[0]=rotateY/r_p_s;

    if(leg==LEFT)
        copyData(m_data+12,t_data,2);
    else
    if(leg==RIGHT)
        copyData(m_data+5,t_data,2);
    else
        mlog.log(ERROR,"setFoot","wrong side");
    updataParam(leg);
    //std::cout<<m_array;
}
void robotModel::setHand(double rotateX,double rotateY,double len,int hand_side)
 {
    if(len<hand_length/3||len>hand_length*2)
        {
            char str[20];sprintf(str,"wrong length:%.4f",len);
            mlog.log(ERROR,"setHand",str);
        }

    int data[3];
    double t_a=acos(len/leg_length/2);

    if(hand_side==LEFT)
        data[1]=(t_a+rotateX)/r_p_s;
    else
        data[1]=(-t_a+rotateX)/r_p_s;
    data[2]=2*t_a/r_p_s;
    data[0]=rotateY/r_p_s;
    mlog<<data[0]<<","<<data[1]<<","<<data[2]<<"\n";
    if(hand_side==LEFT)
    {
        m_data[7]=data[0];
        m_data[14]=-data[1];
        m_data[15]=data[2];

    }else
    if(hand_side==RIGHT)
    {
        m_data[0]=-data[0];
        m_data[16]=-data[1];
        m_data[17]=-data[2];
    }else
    {
        mlog.log(ERROR,"setHand","wrong side");
    }

 }
 void robotModel::setHead(double rotateY,double rotateZ)
 {
     m_data[19]=rotateZ/r_p_s;
     m_data[18]=rotateY/r_p_s;
 }
void robotModel::modifyData(int *d)
{
    int i=0;
    while(i++<20)
        m_data[i-1]=d[i-1];
}
void robotModel::getData(int *d)
{
    int i=0;
    while(i++<20)
        d[i-1]=m_data[i-1];
}
void robotModel::showParam(int leg)
{

    if(leg==LEFT)
    {
        printf("rx,ry,rz,len:%.2f,%.2f,%.2f,%.2f\n",m_info.left_leg[0],m_info.left_leg[1],m_info.left_leg[2],m_info.left_leg[3]);
        printf("rx,ry:%.2f,%.2f\n ",m_info.left_foot[0],m_info.left_foot[1]);
    }
    if(leg==RIGHT)
    {
        printf("rx,ry,rz,len:%.2f,%.2f,%.2f,%.2f\n",m_info.right_leg[0],m_info.right_leg[1],m_info.right_leg[2],m_info.right_leg[3]);
        printf("rx,ry:%.2f,%.2f\n ",m_info.right_foot[0],m_info.right_foot[1]);
    }

    m_leg_vec_left=Map<Vector4f>(m_info.left_leg);
    m_leg_vec_right=Map<Vector4f>(m_info.right_leg);

    m_hand_vec_left=Map<Vector3f>(m_info.left_hand);
    m_hand_vec_right=Map<Vector3f>(m_info.right_hand);
    std::cout<<"leg_left:"<<m_leg_vec_left.adjoint()<<std::endl;
    std::cout<<"leg_right:"<<m_leg_vec_right.adjoint()<<std::endl;
    std::cout<<"hand_left:"<<m_hand_vec_left.adjoint()<<std::endl;
    std::cout<<"hand_r:"<<m_hand_vec_right.adjoint()<<std::endl;
}
void robotModel::updataParam(int leg)
{
    double rotateX,rotateY,rotateZ,len;
    double h=0;
    int t_data[4];
    if(leg==1)
        copyData(t_data,m_data+8,4,1);
    else if(leg==2)
        copyData(t_data,m_data+1,4,-1);
    else
    {
    ;
    }
    rotateX=t_data[1]*r_p_s;
    rotateZ=t_data[2]*r_p_s;
    rotateY=(t_data[0]+t_data[3])/2*r_p_s;
    len=cos((t_data[0]-t_data[3])/2*r_p_s)*leg_length;
    h=leg_length*(cos(t_data[0]*r_p_s)+cos(t_data[3]*r_p_s));
    //printf("angle:%.2f ,%.2f ,%.2f len:%.2f h:%.2f\n",rotateX,rotateY,rotateZ,len,h);

    int e=0;
    if(leg==LEFT)
    {
        m_info.left_leg[0]=-rotateX;
        m_info.left_leg[1]=rotateY;
        m_info.left_leg[2]=-rotateZ;
        m_info.left_leg[3]=len;
        m_info.left_foot[0]=-m_data[13]*r_p_s;
        m_info.left_foot[1]=m_data[12]*r_p_s;

        e=12;
    }
    if(leg==RIGHT)
    {
        m_info.right_leg[0]=rotateX;
        m_info.right_leg[1]=rotateY;
        m_info.right_leg[2]=rotateZ;
        m_info.right_leg[3]=len;
        m_info.right_foot[0]=-m_data[6]*r_p_s;
        m_info.right_foot[1]=-m_data[5]*r_p_s;

        e=5;
    }
     //printf("angle:%.2f ,%.2f\n",r_p_s*m_data[e+1],r_p_s*m_data[e]);
}
void robotModel::test()
{
    static int t_count=0;
    if(t_count++==0)
        cout<<"\
        test robotModel\n\
        enter leginfo ,this program will change Tilt to balance\n";


    float x,y,z,len;
    int leg;
    cin>>x>>y>>z>>len>>leg;
    setLeg(x,y,z,len,leg);
//    cout<<"set rotation of body is:"<<calTilt()<<endl;

}
void robotModel_threePoint::init()
{
    h=  8.1;d=3.5;Mdm= 2.4;
    m_rx=m_ry=0;
    m_rx_fixed=m_ry_fixed=0;
    p_body=initPoseArray(Vector3f(0,0,h));
    p_hip_left=initPoseArray(Vector3f(0,d,0));
    p_foot_left=initPoseArray(Vector3f(0,d,0)+Vector3f(0,0,-leg_length));

    p_hip_right=initPoseArray(Vector3f(0,-d,0));
    p_foot_right=initPoseArray(Vector3f(0,-d,0)+Vector3f(0,0,-leg_length));

    setLeg(0,0,0,28,RIGHT);
    setLeg(0,0,0,28,LEFT);


};
void robotModel_threePoint::setParam(double rx,double ry)
{
    m_rx=rx,m_ry=ry;
    is_new=1;
    setLeg(m_info.left_leg[0],m_info.left_leg[1],m_info.left_leg[2],m_info.left_leg[3],LEFT);
    //double dl=5.6*sin(rx);
    setLeg(m_info.right_leg[0],m_info.right_leg[1],m_info.right_leg[2],m_info.right_leg[3],RIGHT);

}
void robotModel_threePoint::fixParam(double rx,double ry)
{
    m_rx_fixed=rx,m_ry_fixed=ry;
    is_new=1;
    setLeg(m_info.left_leg[0],m_info.left_leg[1],m_info.left_leg[2],m_info.left_leg[3],LEFT);
    //double dl=5.6*sin(rx);
    setLeg(m_info.right_leg[0],m_info.right_leg[1],m_info.right_leg[2],m_info.right_leg[3],RIGHT);

}
double robotModel_threePoint::calTilt(double dy,int leg)
{
    double precision=0.01;
    double begin=-1.2,end=1.2;
    setParam((begin+end)/2,0);
    double t=cal_referCentroid(leg)[1]-dy;
    int t_cout=0;
    while(abs(t)>=precision||t_cout<10)
    {
        t_cout++;
        if(t_cout>15)
        {
            cout<<"error:referCentroid:"<<t<<endl;
            return (begin+end)/2;
        }

        if(t<0)
            end=(begin+end)/2;
        if(t>0)
            begin=(begin+end)/2;
        setParam((begin+end)/2,0);
        t=cal_referCentroid(leg)[1]-dy;
    }
    return (begin+end)/2;
}

//void robotModel_threePoint::setZMP_CM(double cm,double zmp)
//{
//    double precision=0.01;
//    double r1s=-0.6,r1e=0.6,r2s=-0.6,r2e=0.6;
//    double t;
//    setLeg((r1s+r1e)/2,0,0,20,RIGHT);
//    int t_cout=0;
//    double dy=0;
//
//    while((abs(dy)>=precision )||t_cout<10)
//    {
//        t_cout++;
//        if(t_cout>15)
//        {
//            cout<<"error:setZMP_CM:"<<t<<endl;
//            break;
//        }
//        t=calTilt(cm);
//        dy=getPose(p_hip_right+p_foot_right)(1)-zmp;
//        if(dy<0)
//            r1s=(r1s+r1e)/2;
//        if(dy>0)
//            r1e=(r1s+r1e)/2;
//        setLeg((r1s+r1e)/2,0,0,20,RIGHT);
//    }
//   // cout<<"r1:"<<(r1s+r1e)/2<<"r2:"<<t<<endl;
//}

Vector3f  robotModel_threePoint::cal_referCentroid(int leg)
{
    Vector3f t;
    if(is_new){
    p_body=setPoseArray((RY(m_ry)*RX(m_rx)).matrix(),Vector3f(0,0,0))*initPoseArray(Vector3f(0,0,h));

   // cout<<"left hip:"<<endl;
    p_hip_left=setPoseArray(RY(m_ry)*RX(m_rx).matrix(),Vector3f(0,0,0))*initPoseArray(Vector3f(0,d,0));
    p_hip_right=setPoseArray(RY(m_ry)*RX(m_rx).matrix(),Vector3f(0,0,0))*initPoseArray(Vector3f(0,-d,0));
    //cout<<p_hip_left<<endl;

    //cout<<"left foot:"<<endl;
    p_foot_left=initPoseArray(Vector3f(0,0,-m_info.left_leg[3]));
    //relative rotate
    p_foot_left=setPoseArray(AngleAxisf(-m_info.left_leg[2],p_foot_left.block<3,1>(0,2)).matrix(),Vector3f(0,0,0))*p_foot_left;
    p_foot_left=setPoseArray(AngleAxisf(m_info.left_leg[0],p_foot_left.block<3,1>(0,0)).matrix(),Vector3f(0,0,0))*p_foot_left;
    p_foot_left=setPoseArray(AngleAxisf(m_info.left_leg[1],p_foot_left.block<3,1>(0,1)).matrix(),Vector3f(0,0,0))*p_foot_left;

    p_foot_right=initPoseArray(Vector3f(0,0,-m_info.right_leg[3]));
    //relative rotate
    p_foot_right=setPoseArray(AngleAxisf(-m_info.right_leg[2],p_foot_right.block<3,1>(0,2)).matrix(),Vector3f(0,0,0))*p_foot_right;
    //cout<<p_foot_right<<"-1"<<endl;
    p_foot_right=setPoseArray(AngleAxisf(m_info.right_leg[0],p_foot_right.block<3,1>(0,0)).matrix(),Vector3f(0,0,0))*p_foot_right;
    //cout<<p_foot_right<<"-2"<<endl;
    p_foot_right=setPoseArray(AngleAxisf(m_info.right_leg[1],p_foot_right.block<3,1>(0,1)).matrix(),Vector3f(0,0,0))*p_foot_right;
    is_new=0;
    }
    //cout<<p_foot_right<<"-3"<<endl;
    t=(getPose(p_foot_left+p_foot_right)/2+getPose(p_body)*Mdm)/(2+Mdm);
    //cout<<t;
    if(leg==LEFT)
    {
        t=t-getPose(p_foot_left+p_hip_left);
    }
    if(leg==RIGHT)
        t=t-getPose(p_foot_right+p_hip_right);
    return t;
}
void robotModel_threePoint::setLeg(double rotateX,double rotateY,double rotateZ,double len,int leg)
{
    is_new=1;

    if(len<11.4||len>33)

    {
        mlog.log(ERROR,"three_point setleg","out range");
    }

    robotModel::setLeg((rotateX-m_rx-m_rx_fixed),rotateY,rotateZ,len,leg);
    robotModel::setFoot(-rotateX,m_ry+m_ry_fixed,leg);

      if(leg==LEFT)
    {
        m_info.left_leg[0]=rotateX;
        m_info.left_leg[1]=rotateY;
        m_info.left_leg[2]=rotateZ;
        m_info.left_leg[3]=len;
    }
    if(leg==RIGHT)
    {
        m_info.right_leg[0]=rotateX;
        m_info.right_leg[1]=rotateY;
        m_info.right_leg[2]=rotateZ;
        m_info.right_leg[3]=len;
    }
}
Vector3f robotModel_threePoint::calHip(int leg)
{
    Vector3f t;
    if(is_new)
        cal_referCentroid(0);
    t=Mdm*getPose(p_body);
    if(leg==RIGHT)
    {
        t=(t+getPose(p_foot_left/2+p_hip_left))/(Mdm+1);
        t=t-getPose(p_hip_right);
    }
    if(leg==LEFT)
    {
        t=(t+getPose(p_foot_right/2+p_hip_right))/(Mdm+1);
        t=t-getPose(p_hip_left);
    }
    return t;
}
void robotModel_threePoint::test()
{
    static int t_count=0;
    if(t_count++==0)
        cout<<"\
        test robotModel_threePoint\n\
        enter leginfo ,this program will change Tilt to balance\n";


    float x,y,z,len;
    int leg;
    cin>>x>>y>>z>>len>>leg;
    setLeg(x,y,z,len,leg);
    cout<<"set rotation of body is:"<<calTilt()<<endl;

}
